import asyncio
from utils.logger import logger
from px4.drone_controller import DroneController
from config import Config
import time

class StatusMonitor:
    _instance = None  # 单例实例

    def __new__(cls, *args, **kwargs):
        # 如果实例不存在，则创建新实例
        if cls._instance is None:
            cls._instance = super(StatusMonitor, cls).__new__(cls)
        return cls._instance

    def __init__(self, drone, mqtt_client):
        # 防止重复初始化
        if hasattr(self, "_initialized"):
            return
        self._initialized = True

        self.drone = drone
        self.mqtt_client = mqtt_client
        self.status = {
            "gpsInfo": None,
            "position": None,
            "battery": None,
            "flightMode": None,
            "velocity": None,
            "attitude": None,
            "arrmed": None,
            "health": None
        }
        self.push_interval = 0.5  # 2Hz 推送频率，间隔 0.5 秒

    async def monitor_gps(self):
        # 订阅 gps 信息
        async for gps_info in self.drone.telemetry.gps_info():
            self.status["gpsInfo"] = {
                "numSatellites": gps_info.num_satellites,
                "fixType": gps_info.fix_type.name
            }

    async def monitor_position(self):
        # 订阅位置信息
        async for position in self.drone.telemetry.position():
            self.status["position"] = {
                "latitude": position.latitude_deg,
                "longitude": position.longitude_deg,
                "absoluteAltitude": position.absolute_altitude_m,
                "relativeAltitude": position.relative_altitude_m
            }

    async def monitor_battery(self):
        # 订阅电池信息
        async for battery in self.drone.telemetry.battery():
            self.status["battery"] = {
                "remainingPercent": battery.remaining_percent,
                "voltage": battery.voltage_v
            }

    async def monitor_flight_mode(self):
        # 订阅飞行模式信息
        async for flight_mode in self.drone.telemetry.flight_mode():
            self.status["flightMode"] = flight_mode.name

    async def monitor_velocity_ned(self):
        # 订阅飞行速度信息
        async for velocity_ned in self.drone.telemetry.velocity_ned():
            self.status["velocity"] = {
                "north": velocity_ned.north_m_s,
                "east": velocity_ned.east_m_s,
                "down": velocity_ned.down_m_s
            }

    async def monitor_attitude(self):
        # 订阅姿态信息
        async for attitude in self.drone.telemetry.attitude_euler():
            self.status["attitude"] = {
                "roll": attitude.roll_deg,
                "pitch": attitude.pitch_deg,
                "yaw": attitude.yaw_deg
            }

    async def monitor_armed(self):
        # 订阅无人机锁定信息
        async for arrmed in self.drone.telemetry.armed():
            self.status["arrmed"] = arrmed

    async def monitor_health(self):
        # 订阅健康状态信息
        async for health in self.drone.telemetry.health():
            self.status["health"] = {
                "isGyrometerCalibrationOk": health.is_gyrometer_calibration_ok,
                "isAccelerometerCalibrationOk": health.is_accelerometer_calibration_ok,
                "isMagnetometerCalibrationOk": health.is_magnetometer_calibration_ok,
                "isGlobalPositionOk": health.is_global_position_ok,
                "isHomePositionOk": health.is_home_position_ok,
                "isLocalPositionOk": health.is_local_position_ok,
                "isArmable": health.is_armable
            }

    async def push_osd(self):
        droneController = DroneController()
        """定时推送状态"""
        while True:
            self.status["taskExecuteId"] = droneController.get_task_execute_id()
            osdInfo = {
                "timestamp": int(time.time() * 1000),
                "gateway": Config.SERIAL_NUMBER,
                "data": self.status
            }

            self.mqtt_client.publish_status(osdInfo)
            logger.debug(f"推送状态: {self.status}")
            await asyncio.sleep(self.push_interval)

    async def start_monitoring(self):
        """启动状态监控和推送"""
        await asyncio.gather(
            self.monitor_gps(),
            self.monitor_position(),
            self.monitor_battery(),
            self.monitor_flight_mode(),
            self.monitor_velocity_ned(),
            self.monitor_attitude(),
            self.monitor_armed(),
            self.monitor_health(),
            self.push_osd()
        )